#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "nav_move_base");
    ros::NodeHandle node;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);//订阅move_base操作服务器
    //设置目标点
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    geometry_msgs::Pose pose;
    point.x = 17;
    point.y = 28;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.012;
    quaternion.w = 0.999;
    pose.position = point;
    pose.orientation = quaternion;
    ROS_INFO("waiting movebase server.");
    if(!ac.waitForServer(ros::Duration(30)))//等待操作服务器可用
    {
        ROS_INFO("not connect movebase server.");
        return 1;
    }
    ROS_INFO("connected movebase server success.");
    ROS_INFO("start to navigation.");
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = pose;
    ac.sendGoal(goal);//将目标位姿发送到MoveBaseAction服务器
    bool finished_within_time = ac.waitForResult(ros::Duration(1000));//要求机器人在1000s时间内到达目标点
    if(!finished_within_time)//没有及时到达，放弃目标
    {
        ac.cancelGoal();
        ROS_INFO("time out for arrival goal.");
    }
    else
    {
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("succeess to arrival goal!");
        }
        else
        {
            ROS_INFO("other reason to failed to arrival goal.");
        }
    }
    ros::spin();
    ROS_INFO("movebase kill...");
    return 0;
}

